#!/bin/sh
#
# PX4FMU startup script for test hackery.
#
set +e
set R /
set unit_test_failure 0

set BOARD_RC ${R}etc/init.d/rc.board

uorb start

if rgbled start -X
then
	led_control on -c blue
fi

if sercon
then
	echo "[i] USB interface connected"

	# Try to get an USB console
	nshterm /dev/ttyACM0 &
fi

#
# Try to mount the microSD card.
#
mount -t vfat /dev/mmcsd0 /fs/microsd
if [ $? = 0 ]
then
	echo "[i] card mounted at /fs/microsd"
	# Start playing the startup tune
	tune_control play -t 1 # tune 1 = STARTUP
else
	echo "[i] no microSD card found"
	tune_control play error
fi

#
# Start a minimal system
#

#
# Start the ORB (first app to start)
#
uorb start

#
# Load parameters
#
set PARAM_FILE /fs/microsd/params
if mtd start
then
	set PARAM_FILE /fs/mtd_params
fi

param select $PARAM_FILE
if param load
then
	echo "[param] Loaded: $PARAM_FILE"
else
	echo "[param] FAILED loading $PARAM_FILE"
fi

#
# run boards rc.board if available
#
if [ -f $BOARD_RC ]
then
	echo "Board init: ${BOARD_RC}"
	. $BOARD_RC
fi

if [ -f /etc/extras/px4_io-v2_default.bin ]
then
	set io_file /etc/extras/px4_io-v2_default.bin
fi

if px4io start
then
	echo "PX4IO OK"
else
	set unit_test_failure 1
	set unit_test_failure_list "${unit_test_failure_list} px4io_start"
fi

if px4io checkcrc $io_file
then
	echo "PX4IO CRC OK"
else
	echo "PX4IO CRC failure"
	tune_control play -t 16 # tune 16 = PROG_PX4IO
	if px4io forceupdate 14662 $io_file
	then
		if px4io start
		then
			echo "PX4IO restart OK"
			tune_control play-t 17 # tune 17 = PROG_PX4IO_OK
		else
			echo "PX4IO restart failed"
			tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
			set unit_test_failure 1
			set unit_test_failure_list "${unit_test_failure_list} px4io_flash"
		fi
	else
		echo "PX4IO update failed"
		tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
		set unit_test_failure 1
		set unit_test_failure_list "${unit_test_failure_list} px4io_flash"
	fi
fi

#
# The presence of this file suggests we're running a mount stress test
#
if [ -f /fs/microsd/mount_test_cmds.txt ]
then
	tests mount
fi

. ${R}etc/init.d/rc.sensors

ver all

#
# Run unit tests at board boot, reporting failure as needed.
# Add new unit tests using the same pattern as below.
#

if tests all
then
	echo
	echo "All Unit Tests PASSED"
	led_control on -c green
else
	echo
	echo "Some Unit Tests FAILED"
	led_control on -c red
fi

free
